#include "pathBuilder.hpp"
#include "memoryPool.hpp"

#include <iostream>
#include <glm/gtx/norm.hpp>

class Handler_0 : public ISpaceBuilderHandler
{
public:
    bool ValidCond( const BuilderNode* vol ){ return vol->index > 4; };
    void SplitCallback( const bool cond, BuilderNode* vol ){ splitcalled += 1; };
    void LeafCallback( const bool cond, BuilderNode* vol ){ leafcalled += 1; }; 

    uint32_t splitcalled = 0;
    uint32_t leafcalled = 0;
};

class Handler_1 : public ISpaceBuilderHandler
{
public:
    bool ValidCond( const BuilderNode* vol ){ return vol->index > 1 && vol->index < 10; };
    void SplitCallback( const bool cond, BuilderNode* vol ){ splitcalled += 1; };
    void LeafCallback( const bool cond, BuilderNode* vol ){ leafcalled += 1; }; 

    uint32_t splitcalled = 0;
    uint32_t leafcalled = 0;
};

class Handler_2 : public ISpaceBuilderHandler
{
public:
    bool ValidCond( const BuilderNode* vol ){ return vol->index > 1; };
    void SplitCallback( const bool cond, BuilderNode* vol ){ splitcalled += 1; };
    void LeafCallback( const bool cond, BuilderNode* vol ){ leafcalled += 1; }; 

    uint32_t splitcalled = 0;
    uint32_t leafcalled = 0;
};

class Handler_3 : public ISpaceBuilderHandler
{
public:
    bool ValidCond( const BuilderNode* vol ){ return vol->index > 0; };
    void SplitCallback( const bool cond, BuilderNode* vol ){};
    void LeafCallback( const bool cond, BuilderNode* vol ){}; 
};

class Handler_4 : public ISpaceBuilderHandler
{
public:
    bool ValidCond( const BuilderNode* vol )
    {
        uint32_t i = vol->index;
        return i == 1 || i == 3 || (i >= 4 && i < 8) || (i >= 16 && i < 20); };
    void SplitCallback( const bool cond, BuilderNode* vol ){};
    void LeafCallback( const bool cond, BuilderNode* vol ){}; 
};

bool L_PruneFunction( const BuilderNode* node )
{ return (node->mask & FLAG_VALIDVOXEL) == 0; }

bool testCreateBuilderTree()
{
    Handler_0 handler;

    BuilderNode* root = new BuilderNode();
    root->voxel.center = glm::vec3(0.0f, 0.0f, 0.0f);
    root->voxel.size = 2.0f;
    root->index = 0;

    uint32_t id = 1;
    CreateBuilderTree( root, 1.0f, id, &handler );

    if( ( root->mask & FLAG_VALIDVOXEL ) == 0 )
    {
        std::cout << "TestCreateBuilderTree.Test_0: expected root to be valid. " << std::endl;
        return false;
    }

    bool pass = true;
    for(uint32_t i = 0; pass && i < 8; ++i )
    {
        bool valid = ( root->children[i]->mask & FLAG_VALIDVOXEL ) != 0;
        pass &= ( !valid && root->children[i]->index <= 4 )
             || ( valid && root->children[i]->index > 4) ;
    }
    if( !pass )
    {
        std::cout << "TestCreateBuilderTree.Test_1: failed children validation check. " << std::endl;
        return false;
    }

    DeleteBuilderTree(root);
    std::cout << "TestCreateBuilderTree TEST OK.";
    return true;
}

bool testCreateBuilderRoot()
{
    Handler_0 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(2.0f, 1.0f, 2.0f), 1.0f, &handler );
    PruneBuilderRoot( root, L_PruneFunction );
    if( root.childrenCnt != 4 )
    {
        std::cout << "TestCreateBuilderRoot.Test_0: expected 4, but got: " << root.childrenCnt << std::endl;
        return false;
    }

    bool res = true;
    for( uint32_t i = 0; res && i < root.childrenCnt; ++i )
    { res = res && root.children[i] == nullptr; }
    if( !res )
    {
        std::cout << "TestCreateBuilderRoot.Test_1: all children node should be nullptr. " << std::endl;
        return false; 
    }

    BuilderNode* v0 = root.children[0];
    if( v0 != nullptr )
    {
        std::cout << "TestCreateBuilderRoot.Test_2: Search return nullptr. " << std::endl;
        return false;
    }

    BuilderNode* v3 = root.children[3];
    if( v3 != nullptr )
    {
        std::cout << "TestCreateBuilderRoot.Test_3: Search return nullptr. " << std::endl;
        return false;
    }

    DeleteBuilderRoot( root );
    std::cout << "TestCreateBuilderRoot TEST OK.";
    return true;
}

bool testFindBuilderNodeCnt()
{
    Handler_1 handler;
    bool(*pruneFunc)(const BuilderNode* tar) = [](const BuilderNode* tar){ return (tar->mask & FLAG_VALIDVOXEL) == 0; };

    uint32_t nodeCnt = 0;

    BuilderNode* node = new BuilderNode();
    node->voxel.center = glm::vec3();
    node->voxel.size = 4.0f;
    node->index = 0;
    
    uint32_t index = 1;
    CreateBuilderTree( node, 1.0f, index, &handler);
    PruneBuilderTree( node, pruneFunc );
    FindBuilderNodeCnt( node, nodeCnt );
    if( nodeCnt != 10 )
    {
        std::cout << "TestFindBuilderNodeCnt.Test_0: expected node cnt 10, but got: " << nodeCnt << std::endl;
        return false; 
    }

    BuilderNode** list = MemoryPool::AllocArray<BuilderNode*>( nodeCnt );
    index = 0;
    ListBuilderTree( node, list, index );
    if( index != 10 )
    {
        std::cout << "TestFindBuilderNodeCnt.Test_1: expected end index: 10, but got: " << index << std::endl;
        return false; 
    }

    const uint32_t idres[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    bool res = true;
    for( uint32_t i = 0; res && i < nodeCnt; ++i )
    {
        res = res && idres[i] == list[i]->index;
    }
    if( !res )
    {
        std::cout << "TestFindBuilderNodeCnt.Test_2: Node indices mismatch with expected list. " << std::endl;
        return false; 
    }

    index = 0;
    ListBuilderTree( node, list, index, true );
    res = true;
    for( uint32_t i = 0; res && i < nodeCnt; ++i )
    { res = res && i == list[i]->index; }
    if( !res )
    {
        std::cout << "TestFindBuilderNodeCnt.Test_3: Index error when Listing nodes with index resetting. " << std::endl;
        return false; 
    }

    MemoryPool::DeallocArray<BuilderNode*>( list, nodeCnt );
    DeleteBuilderTree( node );
    std::cout << "TestFindBuilderNodeCnt TEST OK.";
    return true;
}

bool testSetupBuilderProxies()
{
    Handler_1 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    const uint32_t nodeCnt = FindBuilderRootCnt( root );
    if( nodeCnt != 9 )
    {
        std::cout << "testSetupBuilderProxies.Test_0: expected node cnt: 9, but got: " << nodeCnt << std::endl;
        return false; 
    }

    BuilderNode* lists[nodeCnt];
    ListBuilderRoot( root, lists );
    const uint32_t idres[] = {0, 4, 5, 6, 7, 8, 9, 2, 3};
    bool res = true;
    for( uint32_t i = 0; res && i < nodeCnt; ++i )
    { res = res && idres[i] == lists[i]->index; }
    if( !res )
    {
        std::cout << "testSetupBuilderProxies.Test_1: Node indices mismatch with expected list. " << std::endl;
        return false; 
    }

    res = true;
    BuilderNode* n0 = lists[0];
    const uint32_t n0Res[] = { UINT32_MAX, UINT32_MAX, UINT32_MAX, 2, UINT32_MAX, UINT32_MAX };
    for( uint32_t i = 0; res && i < 6; ++i )
    {
        BuilderNode* p = n0->proxy[i];
        res = res && p == nullptr ? UINT32_MAX == n0Res[i] : p->index == n0Res[i];
    }
    if( !res )
    {
        std::cout << "testSetupBuilderProxies.Test_2: Failed to pass proxy test for n0 ";
        return false;
    }

    res = true;
    BuilderNode* n1 = lists[1];
    const uint32_t n1Res[] = { 6, UINT32_MAX, UINT32_MAX, 8, UINT32_MAX, 5 };
    for( uint32_t i = 0; res && i < 6; ++i )
    {
        BuilderNode* p = n1->proxy[i];
        res = res && p == nullptr ? UINT32_MAX == n1Res[i] : p->index == n1Res[i];
    }
    if( !res )
    {
        std::cout << "testSetupBuilderProxies.Test_3: Failed to pass proxy test for n1 ";
        return false;
    }

    res = true;
    BuilderNode* n8 = lists[5];
    const uint32_t n8Res[] = { UINT32_MAX, UINT32_MAX, 4, 2, UINT32_MAX, 9 };
    for( uint32_t i = 0; res && i < 6; ++i )
    {
        BuilderNode* p = n8->proxy[i];
        res = res && p == nullptr ? UINT32_MAX == n8Res[i] : p->index == n8Res[i];
    }
    if( !res )
    {
        std::cout << "testSetupBuilderProxies.Test_4: Failed to pass proxy test for n8 ";
        return false;
    }

    DeleteBuilderRoot( root );
    std::cout << "TestSetupBuilderProxies TEST OK.";
    return true;
}

bool testSearchBuilderNode()
{
    Handler_2 handler;

    BuilderNode* testNode = new BuilderNode();
    testNode->voxel.center = glm::vec3();
    testNode->voxel.size = 4.0f;
    testNode->index = 0;
    testNode->mask = 0;
    uint32_t id = 1;
    CreateBuilderTree( testNode, 1.0f, id, &handler );

    if( handler.splitcalled != 2 )
    {
        std::cout << "TestSearchBuilderNode.Test_0: expected tree split time: 2, but got: " << handler.splitcalled << std::endl;
        return false; 
    }
    if( handler.leafcalled != 15 )
    {
        std::cout << "TestSearchBuilderNode.Test_1: expected tree split time: 15, but got: " << handler.splitcalled << std::endl;
        return false; 
    }

    uint32_t resIndex = 0;
    BuilderNode* searchRes[8];

    bool(*leafFunc)(const BuilderNode* tar) = [](const BuilderNode* tar){ return (tar->mask & FLAG_VALIDVOXEL) != 0; };
    for( float x = -1.0f; x < 2.0f; x += 2.0f )
    {
        for( float y = -1.0f; y < 2.0f; y += 2.0f )
        {
            for( float z = -1.0f; z < 2.0f; z += 2.0f )
            {
                searchRes[resIndex] = SearchBuilderNode( glm::vec3(x, y, z), testNode, leafFunc );
                resIndex += 1;
            }
        }
    }

    bool res = true;
    for( uint32_t i = 0; i < 8; ++i )
    {
        BuilderNode* node = searchRes[i];
        res = res && node != nullptr && node->index == 9 + i && node->voxel.size == ( i == 0 ? 1.0f : 2.0f);
    }
    if( !res )
    {
        std::cout << "TestSearchBuilderNode.Test_3: Node search test failed." << std::endl;
        return false; 
    }

    DeleteBuilderTree(testNode);
    std::cout << "TestSearchVoxel TEST OK.";
    return true;
}

bool testSearchBuilderRoot()
{
    Handler_1 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 4.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );

    uint32_t resIndex = 0;
    BuilderNode* searchRes_0[4];
    bool(*leafFunc)(const BuilderNode* tar) = [](const BuilderNode* tar){ return (tar->mask & FLAG_VALIDVOXEL) != 0; };
    for( float x = -2.0f; x < 4.0f; x += 4.0f )
    {
        for( float z = -2.0f; z < 4.0f; z += 4.0f )
        {
            searchRes_0[resIndex] = SearchBuilderRoot(  glm::vec3(x, 0.0f, z), root, leafFunc );
            resIndex += 1;
        }
    }
    
    if( searchRes_0[0] != nullptr )
    {
        std::cout << "TestSearchBuilderRoot.Test_0: search result on 0 should be nullptr." << std::endl;
        return false; 
    }
    if( searchRes_0[1] != nullptr )
    {
        std::cout << "TestSearchBuilderRoot.Test_1: search result on 1 should be nullptr." << std::endl;
        return false; 
    }
    if( searchRes_0[2] == nullptr || searchRes_0[2]->index != 2 )
    {
        std::cout << "TestSearchBuilderRoot.Test_2: search result on 2 should be node whith index 2." << std::endl;
        return false; 
    }

    DeleteBuilderRoot(root);

    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    const uint32_t nodeCnt =  FindBuilderRootCnt( root );
    BuilderNode* list[nodeCnt];
    ListBuilderRoot( root, list );

    const uint32_t indexRes_1[] = {0, 4, 5, 6, 7, 8, 9, 2, 3};
    bool res = true;
    for( uint32_t i = 0; res && i < nodeCnt; ++i )
    { res = res && indexRes_1[i] == list[i]->index; }
    if( nodeCnt != 9 || !res )
    {
        std::cout << "TestSearchBuilderRoot.Test_3: root build is not correct." << std::endl;
        return false; 
    }


    resIndex = 0;
    BuilderNode* searchRes_1[32];
    for( float x = -3.0f; x < 4.0f; x += 2.0f )
    {
        for( float y = -1.0f; y < 2.0f; y += 2.0f )
        {
            for( float z = -3.0f; z < 4.0f; z += 2.0f )
            {
                searchRes_1[resIndex] = SearchBuilderRoot(  glm::vec3(x, y, z), root, leafFunc );
                resIndex += 1;
            }
        }
    }

    uint32_t indexRes_2[] =
    {
        4, 5, UINT32_MAX, UINT32_MAX,
        6, 7, UINT32_MAX, UINT32_MAX,
        8, 9, UINT32_MAX, UINT32_MAX,
        UINT32_MAX, UINT32_MAX, UINT32_MAX, UINT32_MAX,
        2, 2, 3, 3,
    };

    res = true;
    for( uint32_t i = 0; res && i < 20; ++i )
    { res = res && ( searchRes_1[i] == nullptr ? indexRes_2[i] == UINT32_MAX : indexRes_2[i] == searchRes_1[i]->index ); }
    if( !res )
    {
        std::cout << "TestSearchBuilderRoot.Test_4: failed to test 20 search points." << std::endl;
        return false; 
    }

    DeleteBuilderRoot(root);
    std::cout << "TestSearchBuilderRoot TEST OK.";
    return true;
}

bool testSetupBuilderConnections()
{
    Handler_1 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    uint32_t nodeCnt = FindBuilderRootCnt( root );
    BuilderNode* list[nodeCnt];
    ListBuilderRoot( root, list );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    const uint32_t expIds[] = { 0, 4, 5, 6, 7, 8, 9, 2, 3 };
    const uint32_t expCons[] = {
        5, 6, 8,
        4, 7, 9,
        4, 7,
        5, 6,
        2, 4, 9,
        2, 5, 8,
        3, 8, 9,
        2,
    };
    uint32_t icon = 0;
    bool res = true;
    for( uint32_t i = 0; res && i < nodeCnt; ++i )
    {
        BuilderNode* n = list[i];
        res = res && n->index == expIds[i];
        uint32_t conSize = n->connectionCnt;
        for( uint32_t j = 0; res && j < conSize; ++j )
        { res = res && n->connections[j]->index == expCons[ icon + j ]; }
        icon += conSize;
    }
    if( !res )
    {
        std::cout << "testSetupBuilderConnections.Test_0: failed to pass builder node connections test." << std::endl;
        return false; 
    }
    DeleteBuilderRoot(root);

    std::cout << "TestSetupBuilderConnections TEST OK.";
    return true;
}

bool testBuildSpaceRoot()
{
    Handler_1 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    SpaceRoot sRoot;
    BuildSpaceRoot( root, sRoot );
    DeleteBuilderRoot( root );

    if( sRoot.nodeCnt != 9 || sRoot.childrenCnt != 4 || sRoot.connectionCnt != 20 )
    {
        std::cout << "TestBuildSpaceRoot.Test_0: expected 9, 4, 20, but got: "
            << sRoot.nodeCnt << ", " << sRoot.childrenCnt << ", " << sRoot.connectionCnt << std::endl;
        return false;
    }

    bool res = true;
    const uint32_t iRes_0[] = { 0, UINT32_MAX, 7, 8 };
    for( uint32_t i = 0; res && i < sRoot.childrenCnt; ++i )
    { res = res && sRoot.children[i] == iRes_0[i]; }
    if( !res )
    {
        std::cout << "TestBuildSpaceRoot.Test_1: failed to check root children indicies." << std::endl;
        return false; 
    }

    SpaceNode* sNode_8 = sRoot.nodes + 8;
    if( sNode_8->connectionCnt != 1 )
    {
        std::cout << "TestBuildSpaceRoot.Test_2: expected 1, but got: " << sNode_8->connectionCnt << std::endl;
        return false; 
    }

    SpaceNode* sNode_7 = sRoot.nodes + 7;
    if( sNode_7->connectionCnt != 3 )
    {
        std::cout << "TestBuildSpaceRoot.Test_3: expected 3, but got: " << sNode_7->connectionCnt << std::endl;
        return false; 
    }

    res = true;
    const uint32_t iRes_1[] = {8, 5, 6};
    for( uint32_t i = 0; res && i < sNode_7->connectionCnt; ++i )
    { res = res && iRes_1[i] == sNode_7->connections[i]; }
    if( !res )
    {
        std::cout << "TestBuildSpaceRoot.Test_4: failed to check connections for node 7." << std::endl;
        return false; 
    }

    DeleteSpaceRoot(sRoot);
    std::cout << "TestBuildSpaceRoot TEST OK.";
    return true;
}

bool testSearchSpaceRoot()
{
    Handler_1 handler;

    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    SpaceRoot sRoot;
    BuildSpaceRoot( root, sRoot );
    DeleteBuilderRoot( root );

    bool(*leafFunc)(const SpaceNode* tar) = [](const SpaceNode* tar){ return (tar->mask & FLAG_LEAF) != 0; };
    uint32_t resIndex = 0;
    SpaceNode* searchRes_1[32];
    for( float x = -3.0f; x < 4.0f; x += 2.0f )
    {
        for( float y = -1.0f; y < 2.0f; y += 2.0f )
        {
            for( float z = -3.0f; z < 4.0f; z += 2.0f )
            {
                searchRes_1[resIndex] = SearchSpaceRoot(  glm::vec3(x, y, z), sRoot, leafFunc );
                resIndex += 1;
            }
        }
    }

    uint32_t indexRes_1[] =
    {
        1, 2, UINT32_MAX, UINT32_MAX,
        3, 4, UINT32_MAX, UINT32_MAX,
        5, 6, UINT32_MAX, UINT32_MAX,
        UINT32_MAX, UINT32_MAX, UINT32_MAX, UINT32_MAX,
        7, 7, 8, 8,
    };

    bool res = true;
    for( uint32_t i = 0; res && i < 20; ++i )
    { res = res && ( ( searchRes_1[i] == nullptr && indexRes_1[i] == UINT32_MAX ) || ( searchRes_1[i] - sRoot.nodes == indexRes_1[i] ) ); }
    if( !res )
    {
        std::cout << "TestSearchSpaceRoot.Test_0: failed to test 20 search points." << std::endl;
        return false; 
    }

    DeleteSpaceRoot(sRoot);
    std::cout << "TestSearchSpaceRoot TEST OK.";
    return true;
}

bool testSelectBuilderArea()
{
    Handler_1 handler;
    BuilderRoot root;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );
    SelectBuilderArea( root, glm::vec3() );
    const uint32_t cnt_0 = FindBuilderRootCnt( root );
    if( cnt_0 != 9 )
    {
        std::cout << "TestSelectBuilderArea.Test_0: expected 9, but got: " << cnt_0 << std::endl;
        return false; 
    }
    DeleteBuilderRoot( root );

    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );
    SelectBuilderArea( root, glm::vec3(0.0f, 10.0f, 0.0f ) );
    const uint32_t cnt_1 = FindBuilderRootCnt( root );
    if( cnt_1 != 0 )
    {
        std::cout << "TestSelectBuilderArea.Test_0: expected 0, but got: " << cnt_1 << std::endl;
        return false; 
    }

    DeleteBuilderRoot( root );
    std::cout << "TestSelectBuilderArea TEST OK.";
    return true;
}

bool testFindPath()
{
    BuilderRoot root;
    SpaceRoot sroot;
    bool(*lfunc)(const SpaceNode* tar) = [](const SpaceNode* tar){ return (tar->mask & FLAG_LEAF) != 0; };
    float(*hfunc) (SpaceNode*, glm::vec3 ) = []( SpaceNode* c, glm::vec3 e ){ return glm::distance2(e, c->voxel.center); };
    Handler_3 handler_3;

    CircularDeque<glm::vec3> wps(8);
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 4.0f, &handler_3);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    BuildSpaceRoot( root, sroot );
    DeleteBuilderRoot(root);

    bool res = true;
    FindPathInSpaceRoot( sroot, glm::vec3(2.0f, 0.0f, -3.0f), glm::vec3(-3.0f, -1.0f, 1.0f), 0.0f, wps, lfunc, hfunc );

    if( wps.Size() != 4 )
    {
        std::cout << "TestFindPath.Test_0: expected 4, but got: " << wps.Size() << std::endl;
        return false; 
    }

    glm::vec3 resWps_0[] = 
    {
        { 2.0f, 0.0f, -3.0f},
        { 0.0f, -0.75f, 0.0f },
        { 0.0f, -0.75f, 0.0f },
        { -3.0f, -1.0f, 1.0f },
    };
    for(uint32_t i = 0; res && i < wps.Size(); ++i )
    { res = res && glm::distance2( resWps_0[i], wps[i] ) < 0.00001f; }
    if( !res )
    {
        std::cout << "TestFindPath.Test_1: Way points does not match expected way points." << std::endl;
        return false; 
    }
    DeleteSpaceRoot(sroot);

    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler_3);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    BuildSpaceRoot( root, sroot );
    DeleteBuilderRoot(root);

    if( sroot.nodeCnt != 12 )
    {
        std::cout << "TestFindPath.Test_2: expected 12, but got: " << sroot.nodeCnt << std::endl;
        return false; 
    }

    FindPathInSpaceRoot( sroot, glm::vec3(2.0f, 0.0f, -3.0f), glm::vec3(-3.0f, -1.0f, 0.0f), 0.0f, wps, lfunc, hfunc );

    if( wps.Size() != 4 )
    {
        std::cout << "TestFindPath.Test_3: expected 4, but got: " << wps.Size() << std::endl;
        return false; 
    }

    glm::vec3 resWps_1[] = 
    {
        { 2.0f, 0.0f, -3.0f},
        { 0.0f, -0.4f, -1.8f },
        { -2.0f, -1.0f, 0.0f },
        { -3.0f, -1.0f, 0.0f },
    };
    for(uint32_t i = 0; res && i < wps.Size(); ++i )
    { res = res && glm::distance2( resWps_1[i], wps[i] ) < 0.00001f; }
    if( !res )
    {
        std::cout << "TestFindPath.Test_4: Way points does not match expected way points." << std::endl;
        return false; 
    }
    DeleteSpaceRoot(sroot);

    Handler_4 hanlder_4;
    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &hanlder_4);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );
    BuildSpaceRoot( root, sroot );
    DeleteBuilderRoot( root );
    if( sroot.nodeCnt != 12 )
    {
        std::cout << "TestFindPath.Test_5: expected 12, but got: " << sroot.nodeCnt << std::endl;
        return false; 
    }

    FindPathInSpaceRoot( sroot, glm::vec3(-3.0f, -1.0f, -3.0f), glm::vec3( 3.0f, 1.0f, -3.0f), 0.1f, wps, lfunc, hfunc );

    glm::vec3 resWps_2[] = 
    {
        { -3.0f, -1.0f, -3.0f},
        { -2.1f, -0.1f, -3.0f },
        { -2.1f, 1.0f, -2.1f },
        { -2.1f, 1.0f, -0.1f },
        { -0.1f, 1.0f, 0.1f},
        { 2.1f, 1.0f, -0.1f },
        { 2.68966f, 1.0f, -1.9f },
        { 3.0f, 1.0f, -3.0f },
    };
    for(uint32_t i = 0; res && i < wps.Size(); ++i )
    { res = res && glm::distance2( resWps_2[i], wps[i] ) < 0.00001f; }
    if( !res )
    {
        std::cout << "TestFindPath.Test_6: Way points does not match expected way points." << std::endl;
        return false; 
    }
    DeleteSpaceRoot( sroot );

    std::cout << "TestFindPath TEST OK.";
    return true;
}

bool testSerializeSpaceInfo()
{
    bool res = true; 

    Handler_2 handler;
    BuilderRoot root;
    SpaceRoot sroot_0;
    SpaceRoot sroot_1;

    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    BuildSpaceRoot( root, sroot_0 );
    DeleteBuilderRoot( root );

    char* datum = nullptr;
    uint32_t size = 0; 
    SerializeSpaceInfo( sroot_0, datum, size );
    DeserializeSpaceInfo( sroot_1, datum, size );

    if( size != 1490 )
    {
        std::cout << "TestSerializeSpaceInfo.Test_0: exptected 1490, but got: " << size << std::endl;
        return false; 
    }
    if( sroot_1.nodeCnt != 20 )
    {
        std::cout << "TestSerializeSpaceInfo.Test_1: exptected 20, but got: " << sroot_1.nodeCnt << std::endl;
        return false; 
    }
    if( sroot_0.nodeCnt != sroot_1.nodeCnt )
    {
        std::cout << "TestSerializeSpaceInfo.Test_2: exptected node cnt to equal. " << std::endl;
        return false; 
    }
    if( sroot_0.connectionCnt != 74 || sroot_0.connectionCnt != sroot_1.connectionCnt )
    {
        std::cout << "TestSerializeSpaceInfo.Test_3: exptected conneciton cnt to equal and is 74, but got: "
            << sroot_0.connectionCnt << std::endl;
        return false; 
    }
    for( uint32_t i = 0; res && i < sroot_0.connectionCnt; ++i )
    { res = res && sroot_0.connections[i] == sroot_1.connections[i]; }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_4: failed to pass connection array check. " << std::endl;
        return false; 
    }
    for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
    { res = res && sroot_0.nodes[i].mask == sroot_1.nodes[i].mask; }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_5: failed to pass node flag check. " << std::endl;
        return false; 
    }
    for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
    {
        res = res
            && glm::abs(sroot_0.nodes[i].voxel.size - sroot_1.nodes[i].voxel.size) < 0.00001
            && glm::distance2( sroot_0.nodes[i].voxel.center, sroot_1.nodes[i].voxel.center) < 0.00001;
    }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_6: failed to pass node voxel check. " << std::endl;
        return false; 
    }
    if( sroot_0.childrenCnt != sroot_1.childrenCnt )
    {
        std::cout << "TestSerializeSpaceInfo.Test_7: failed to check root node number. " << std::endl;
        return false;
    }
    for( uint32_t i = 0; res && i < sroot_0.childrenCnt; ++i )
    {
        res = res && sroot_0.children[i] == sroot_1.children[i];
    }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_8: failed to check root nodes. " << std::endl;
        return false; 
    }

    for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
    {
        SpaceNode* n1 = sroot_0.nodes + i;
        SpaceNode* n2 = sroot_1.nodes + i;
        for( uint32_t j = 0; res && j < 8; ++j )
        { res = res && n1->children[j] == n2->children[j]; }
    }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_9: failed to pass node children check. " << std::endl;
        return false; 
    }

    for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
    {
        SpaceNode* n1 = sroot_0.nodes + i;
        SpaceNode* n2 = sroot_1.nodes + i;
        res = res && n1->connectionCnt == n2->connectionCnt;
    }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_10: failed to pass node connection count check. " << std::endl;
        return false; 
    } 

    for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
    {
        SpaceNode* n1 = sroot_0.nodes + i;
        SpaceNode* n2 = sroot_1.nodes + i;
        uint32_t cnt = n1->connectionCnt;
        for( uint32_t j = 0; res && j < cnt; ++j )
        { res = res && n1->connections[j] == n2->connections[j]; }
    }
    if( !res )
    {
        std::cout << "TestSerializeSpaceInfo.Test_11: failed to pass connection check. " << std::endl;
        return false; 
    } 

    delete [] datum;
    DeleteSpaceRoot( sroot_0 );
    DeleteSpaceRoot( sroot_1 );

    std::cout << "TestSerializeSpaceInfo TEST OK.";
    return true;
}

bool testIOSpaceInfo()
{
    bool res = true;
    Handler_2 handler;
    BuilderRoot root;
    SpaceRoot sroot_0;
    SpaceRoot sroot_1;

    CreateBuilderRoot( root, glm::vec3(), glm::vec3(8.0f, 4.0f, 8.0f), 2.0f, &handler);
    PruneBuilderRoot( root, L_PruneFunction );
    SetupBuilderProxies( root );
    SetupBuilderConnections( root );

    BuildSpaceRoot( root, sroot_0 );
    DeleteBuilderRoot( root );

    try 
    {
        ExportToFile( "data/spaceinfo_0.bin", sroot_0 );
        ImportFromFile( "data/spaceinfo_0.bin", sroot_1 );

        if( sroot_1.nodeCnt != 20 )
        {
            std::cout << "TestIOSpaceInfo.Test_1: exptected 20, but got: " << sroot_1.nodeCnt << std::endl;
            return false; 
        }
        if( sroot_0.nodeCnt != sroot_1.nodeCnt )
        {
            std::cout << "TestIOSpaceInfo.Test_2: exptected node cnt to equal. " << std::endl;
            return false; 
        }
        
        for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
        { res = res && sroot_0.nodes[i].mask == sroot_1.nodes[i].mask; }
        if( !res )
        {
            std::cout << "TestIOSpaceInfo.Test_3: failed to pass node flag check. " << std::endl;
            return false; 
        }

        for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
        {
            res = res
                && glm::abs(sroot_0.nodes[i].voxel.size - sroot_1.nodes[i].voxel.size) < 0.00001
                && glm::distance2( sroot_0.nodes[i].voxel.center, sroot_1.nodes[i].voxel.center) < 0.00001;
        }
        if( !res )
        {
            std::cout << "TestIOSpaceInfo.Test_4: failed to pass node voxel check. " << std::endl;
            return false; 
        }

        for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
        {
            SpaceNode* n1 = sroot_0.nodes + i;
            SpaceNode* n2 = sroot_1.nodes + i;
            for( uint32_t j = 0; res && j < 8; ++j )
            { res = res && n1->children[j] == n2->children[j]; }
        }
        if( !res )
        {
            std::cout << "TestIOSpaceInfo.Test_5: failed to pass node children check. " << std::endl;
            return false; 
        }

        for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
        {
            SpaceNode* n1 = sroot_0.nodes + i;
            SpaceNode* n2 = sroot_1.nodes + i;
            res = res && n1->connectionCnt == n2->connectionCnt;
        }
        if( !res )
        {
            std::cout << "TestIOSpaceInfo.Test_6: failed to pass node connection count check. " << std::endl;
            return false; 
        } 
    
        for( uint32_t i = 0; res && i < sroot_0.nodeCnt; ++i )
        {
            SpaceNode* n1 = sroot_0.nodes + i;
            SpaceNode* n2 = sroot_1.nodes + i;
            uint32_t cnt = n1->connectionCnt;
            for( uint32_t j = 0; res && j < cnt; ++j )
            { res = res && n1->connections[j] == n2->connections[j]; }
        }
        if( !res )
        {
            std::cout << "TestIOSpaceInfo.Test_6: failed to pass connection check. " << std::endl;
            return false; 
        } 

        DeleteSpaceRoot( sroot_0 );
        DeleteSpaceRoot( sroot_1 );
    } catch ( const MemoryBucketAllocError& err )
    {
        std::cout << err.what() << std::endl;
        return false;
    }

    std::cout << "TestIOSpaceInfo TEST OK.";
    return true;
}


int main()
{
    constexpr uint16_t blockSizes[] = { 1031, 509, 257, 167, 83, 47, 23, 11, 5,   2,  1 };
    constexpr uint32_t blockCnts[] =  { 8,    0,   8,   128, 2,  0,  0,  64, 128, 64, 64 };
    MemoryPool::inst()->InitMemoryPool( blockSizes, blockCnts, 11 );

    char mmsg[512];
    bool res = true;
    bool(*funcs[])() = 
    {
        testCreateBuilderTree,
        testCreateBuilderRoot,
        testFindBuilderNodeCnt,
        testSetupBuilderProxies,
        testSearchBuilderNode,
        testSearchBuilderRoot,
        testSetupBuilderConnections,
        testBuildSpaceRoot,
        testSearchSpaceRoot,
        testSelectBuilderArea,
        testFindPath,
        testSerializeSpaceInfo,
        testIOSpaceInfo,
    };
    size_t fncCnt = sizeof(funcs) / sizeof(void*);
    
    clock_t te = 0;
    for(size_t i = 0; i < fncCnt && res; i++)
    {
        clock_t t = clock();
        res = funcs[i]();
        t = clock() - t;
        te += t;
        std::cout << ".........." << t << " ticks. (" << ((float)t)/CLOCKS_PER_SEC << " secs)" << std::endl;
        res = res && MemoryPool::inst()->PoolEmpty();
    }

    MemoryPool::inst()->FetchAllBucketInfo( mmsg, 512, 1 );
    std::cout << std::endl;
    std::cout << "Unittest Dumping memory pool usage summery" << std::endl;
    std::cout << "==================================================================" << std::endl;
    std::cout << mmsg << std::endl;

    if(res){
        std::cout << "======= All Test OK =======" << std::endl;
        std::cout << "Test took total: " << te << " ticks. (" << ((float)te)/CLOCKS_PER_SEC << " secs)" << std::endl;
    }
    return 0;
}